// gain.h
#pragma once
#include <Arduino.h>
#include <ArduinoJson.h>
#include "BluetoothSerial.h"
#include <initializer_list>
#include <utility>
#include <math.h>

BluetoothSerial SerialBT;

// ====== 可调映射参数（只用前三路）======
#define SERVO_PULSE_MIN  100     // server 侧一致：100~599
#define SERVO_PULSE_MAX  599

static int  ADC_MIN3[3]     = { 0, 0, 0 };
static int  ADC_MAX3[3]     = { 4095, 4095, 4095 };
static bool ADC_INVERT3[3]  = { false, false, false };

// 由 send.h 实现
bool sendServoControl(std::initializer_list<std::pair<int,int>> items);

// ================== 引脚 ==================
#define MUX_S0   16
#define MUX_S1   17
#define MUX_S2   18
#define MUX_S3   19
#define MUX_EN   21      // 若 EN 接地，注释掉本行并移除相关代码
#define MUX_SIG  33      // ADC1：32~39，选 33

// 读数相关
#define ADC_BITS         12
#define ADC_MAX_VALUE    ((1 << ADC_BITS) - 1) // 4095
#define ADC_ATTEN        ADC_11db     // 0~3.3V
#define SAMPLE_COUNT     4
#define SETTLE_US        8
#define SEND_CHANNELS    16

inline void toPrint(const String &msg){ Serial.println(msg); SerialBT.println(msg); }
inline void toPrint(int v)            { String s=String(v); Serial.println(s); SerialBT.println(s); }

// ================== 硬件初始化 ==================
inline void initPins() {
  pinMode(MUX_S0, OUTPUT);
  pinMode(MUX_S1, OUTPUT);
  pinMode(MUX_S2, OUTPUT);
  pinMode(MUX_S3, OUTPUT);
  pinMode(MUX_SIG, INPUT);
#ifdef MUX_EN
  pinMode(MUX_EN, OUTPUT);
  digitalWrite(MUX_EN, LOW);  // 低电平使能
#endif
}
inline void setupADC() { analogSetPinAttenuation(MUX_SIG, ADC_ATTEN); analogReadResolution(ADC_BITS); }

static bool _mux_adc_inited = false;
inline void ensureMuxADC(){ if(!_mux_adc_inited){ initPins(); setupADC(); _mux_adc_inited=true; }}

// ================== 采样 ==================
inline void muxSelect(uint8_t ch){
  digitalWrite(MUX_S0, ch & 0x01);
  digitalWrite(MUX_S1, (ch >> 1) & 0x01);
  digitalWrite(MUX_S2, (ch >> 2) & 0x01);
  digitalWrite(MUX_S3, (ch >> 3) & 0x01);
}
inline int readChannel(uint8_t ch, uint8_t samples=SAMPLE_COUNT){
  ensureMuxADC();
  muxSelect(ch);
  delayMicroseconds(SETTLE_US);
  (void)analogRead(MUX_SIG); // 丢弃一次
  uint32_t acc=0; for(uint8_t i=0;i<samples;i++) acc+=analogRead(MUX_SIG);
  return (int)(acc/samples);
}

// ================== ADC→脉宽映射 & 发送（前三路） ==================
static inline int clampi(int v,int lo,int hi){ return v<lo?lo:(v>hi?hi:v); }

static inline int adcToPulse(int adc, int idx3){
  int amin=ADC_MIN3[idx3], amax=ADC_MAX3[idx3];
  if (amax<=amin) return SERVO_PULSE_MIN;
  float t=(float)(adc-amin)/(float)(amax-amin);
  if (t<0) t=0; if(t>1) t=1;
  if (ADC_INVERT3[idx3]) t=1.f - t;
  int pulse=(int)lroundf(SERVO_PULSE_MIN + t * (SERVO_PULSE_MAX - SERVO_PULSE_MIN));
  return clampi(pulse, SERVO_PULSE_MIN, SERVO_PULSE_MAX);
}

/** 读取 0/1/2 -> 映射到脉宽 -> 通过私聊发给目标设备（PCA9685） */
inline void readFirst3AndSendServos(){
  int a0=readChannel(0), a1=readChannel(1), a2=readChannel(2);
  int p0=adcToPulse(a0,0), p1=adcToPulse(a1,1), p2=adcToPulse(a2,2);
  sendServoControl({ {0,p0}, {1,p1}, {2,p2} });
}
